#include <hidef.h>      /* common defines and macros */
#include <string.h>
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include "derivative.h"      /* derivative-specific definitions */
#include "i2c.h"
#include "sci.h"
#include "pll.h"
#include "lcd.h"
#include "pwm.h"
#include "atd.h"
#include "dac.h"

#define PI 3.14159


static volatile int RTIcycles;
static volatile double latitude,longitude,bearingGPS,DOP,speed,DesiredSteering,ActualSteering;
static volatile int X,Y,Trig;
static volatile unsigned char SCI0Index = 0;
static volatile unsigned char SCI0Temp[MAXLENGTH] = {0};
static volatile unsigned char SCI0Buff[MAXLENGTH] = {0};

static volatile unsigned char SCI1Index = 0;
static volatile unsigned char BuffSelect = 0;
static volatile unsigned char SCI1Temp[90] = {0};
static volatile unsigned char GSA[90] = {0};
static volatile unsigned char RMC[90] = {0};

__interrupt void RealTimeInterrupt(void);

void parse_GPS(void);
void parse_joystick(void);

void calc_gps_angle(double,double);

void main(void) {
  int heading = 0;
  uint count = 0;
  char msg[MAXLENGTH], *pmsg;
  char val[15]={0};		//temp value for sprintf
  pmsg = msg;
  RTICTL=0x33;
  CRGINT |= 0x80;
  RTIcycles = 0;
  EnableInterrupts; 
  	
  PLL_Init();
  sci_init();
  i2c_init();
  LCD_init();
  pwm_init();
  atd_init();
  dac_init();
  
  pwm_set(20,0);
  dac_writeValue(128,0);

  /* Initialisation complete, writing startup statements */
  writeLine("Hello World!    ", 0);  // top line
  writeLine("I have a Hand!  ", 1);  // bottom line

  //pmsg = sci_scan(0);
  //print_angle(heading,0);    

  for(;;) {
    if(RTIcycles==976) {      
      //parse_joy_stick(SCI0Buff);
      //heading = i2c_getAngle();
     //sci_print(SCI0Buff,0);
     sprintf(val,"Lat:%f",latitude);
     writeLine(val,0);
     RTIcycles=0;
    } 
  } /* loop forever */
  /* please make sure that you never leave main */
}

void parse_GPS(void) {
  int i;
  strtok(GSA,",");
  for(i=0;i<12;i++) {
    strtok(NULL,",");
  }
  DOP=atof(strtok(NULL,","));
  strtok(RMC,",");
  strtok(NULL,",");
  strtok(NULL,",");
  latitude=atof(strtok(NULL,","));
  strtok(NULL,",");
  longitude=atof(strtok(NULL,","));
  strtok(NULL,",");
  speed=atof(strtok(NULL,","));
  bearingGPS=atof(strtok(NULL,","));
}

void parse_joystick(void) {
  X=atoi(strtok(SCI0Buff,","));
  Y=atoi(strtok(NULL,","));
  Trig=atoi(strtok(NULL,","));
}

void calc_gps_angle(double latT, double longT) {
  double w,d,a,desHead,wheelHead;
 double dL, dLo, toRad;
 double latS, longS, latD, lonD;
 toRad = PI/180;
 dL = (latD - latitude) * toRad;
 dLo = (lonD- longitude) * toRad;
 latD = latT * toRad;
 lonD = longT * toRad;
 latS = latitude * toRad;
 longS = longitude * toRad;
 a = atan2(sin(dLo)*cos(latD), cos(latS)*sin(latD) - sin(latS)*cos(latD)*cos(dLo));  // The azmith or angle relative to north in radians

  w=(cos((90-latT)*PI/180)*cos((90-latitude)*PI/180))+(sin((90-latT)*PI/180)*sin((90-latitude)*PI/180)*cos((longT-longitude)*PI/180));
  d=6378*acos(w);     //distance to target in km
//  a=asin(sin((90-latT)*PI/180)*sin((longT-longitude)*PI/180)/sin(w)); //angle to target in radians
  if (a<=PI) {
    desHead=a+PI;     //desired heading
  } else {
    desHead=a-PI;
  }
  wheelHead=(bearingGPS-ActualSteering)*PI/180;
  
  if ((desHead-wheelHead)*180/PI < -180){
    DesiredSteering = -180;
  } else if ((desHead-wheelHead)*180/PI > 180) {
    DesiredSteering = 180;
  } else {
    DesiredSteering = (desHead-wheelHead)*180/PI;
  }
}

__interrupt VectorNumber_Vrti void RealTimeInterrupt(void)
{
  RTIcycles++;                    /* increment the counter        */
  CRGFLG = 0x80;            /* clear rti flag               */
}

__interrupt VectorNumber_Vsci0 void SCI0Interrupt(void)
{
   unsigned char c;
   
   c=SCI0SR1;
   c=SCI0DRL;
   if (c != 'E') {           //Not end of string
     SCI0Temp[SCI0Index]=c;   //Append character received
     SCI0Index++;
   } else {                   //End of string
     SCI0Temp[SCI0Index]=c;
     copystr(SCI0Temp,SCI0Buff,MAXLENGTH);  //Copy received string from working to read buffer
     //sci_print(SCI0Buff,1);   // For testing
     SCI0Index = 0;              
     zero(SCI0Temp,MAXLENGTH); //Reset and zero out working buffer
   }
}


__interrupt VectorNumber_Vsci1 void SCI1Interrupt(void)
{
   unsigned char c;
   c=SCI1SR1;
   c=SCI1DRL;
   if (c != 13) {             //Not end of string
     SCI1Temp[SCI1Index]=c;     //Append character received
     SCI1Index++;
   } else {                      //End of string
     if (BuffSelect == 0) {
       copystr(SCI1Temp,GSA,90);  //Copy received string from working to GSA buffer
     } else {
       copystr(SCI1Temp,RMC,90);  //Copy received string from working to RMC buffer
       parse_GPS();
     }
     SCI1Index = 0;
     BuffSelect = 1 - BuffSelect;  //Toggle Buffer
     zero(SCI1Temp,90);  //Reset and zero out working buffer
   }
}
